#include "./Camera.h"


Eigen::Vector3d Camera::Projection(Eigen::Vector2d Pi);
Eigen::Vector2d Camera::Projection(Eigen::Vector3d Pw)
{
    Eigen::Vector3d Pc=R_*Pw+t_;
    Eigen::Vector2d Pi={Pc[0]/Pc[2],Pc[1]/Pc[2]};


    double r =Pi[0]*Pi[0]+Pi[1]*Pi[1];
    double radio= 1+r*undistortParams[0]=r*undistortParams[1]*r;
    Pi[0]*=radio;
    Pi[1]*=radio;
    Eigen::Vector2d p ={  a_*f_*Pi[0]+u0,b_*f_*Pi[1]+v0};
    return p ;

};